/* Copyright (C) 2011 Riccardo Marson

   This file is part of OpenPool, an open-source game of pool. 

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
   RICCARDO MARSON BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN
   AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
   CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

  See Readme.rtf for further information.
  
*/

#include "physics.h"
#include <iostream>

/* ************************************
   CBasicPhysics class implementation
  ************************************* */

CPoolPhysics::CPoolPhysics(float simTimeStep, float gravityAcc, float friction, float density )
{
    CLogManager::getSingleton()->logMessage("Initialising Physics Engine...");

    mTimeStep = simTimeStep;
    mGravityAcceleration.SetComponents(0.0f, 0.0f, -gravityAcc);
    mFrictionCoefficient = friction;
    mAirDensity = density;
}

CPoolPhysics::~CPoolPhysics()
{
    CLogManager::getSingleton()->logMessage("Physics Engine Shutting Down.");
}

void CPoolPhysics::ResetEnvironmentVariables(float newTimeStep, CVector3 gravity, float friction, float density )
{
    mTimeStep = newTimeStep;
    mGravityAcceleration = gravity;
    mFrictionCoefficient = friction;
    mAirDensity = density;

}

void CPoolPhysics::MoveObject(IObject *object)
{
    CVector3 x0, v0, vtmp, acc, k1x, k1v, k2x, k2v, k3x, k3v, k4x, k4v, x, v;
    float weights[2];
    weights[0] = 1.0/6.0;
    weights[1] = 1.0/3.0;

    float mass = object->GetMass();
    float cd = object->GetDragCoeff();
    float Ae = object->GetCrossSection();
    bool isFalling = object->IsFalling();

    //propagate 1 step
    x0 = object->GetPosition();
    v0 = object->GetVelocity();

    //rk4 first step
    acc = Derivatives(x0, v0, mass, cd, Ae, isFalling );
    k1x = v0 * mTimeStep;
    k1v = acc * mTimeStep;

    CVector3 xtemp;

    //rk4 second step
    xtemp = (x0 + k1x*0.5);
    vtmp = (v0 + k1v*0.5);
    acc = Derivatives( xtemp , vtmp, mass, cd, Ae, isFalling );
    k2x = vtmp * mTimeStep;
    k2v = acc * mTimeStep;

    //rk4 third step
    xtemp = (x0 + k2x*0.5);
    vtmp = (v0 + k2v*0.5);
    acc = Derivatives( xtemp , vtmp, mass, cd, Ae, isFalling );;
    k3x = vtmp * mTimeStep;
    k3v = acc * mTimeStep;

    //rk4 fourth step
    xtemp = (x0 + k3x);
    vtmp = (v0 + k3v);
    acc = Derivatives( xtemp , vtmp, mass, cd, Ae, isFalling );
    k4x = vtmp * mTimeStep;
    k4v = acc * mTimeStep;

    //final update
    x = x0 + k1x * weights[0] + k2x * weights[1] + k3x * weights[1] + k4x * weights[0];
    v = v0 + k1v * weights[0] + k2v * weights[1] + k3v * weights[1] + k4v * weights[0];

    //always check that, if the ball is not marked as falling, it does not have a vertical
    //velocity and a vertical position different from that of the table plane + its radius
    if (!isFalling)
    {
        if (x.z() != 3.24)
            x.SetComponents(x.x(), x.y(), 3.24);

        if (v.z() != 0.0)
            v.SetComponents( v.x(), v.y(), 0.0 );
    }

    // update position and velocity
    // control!

    x0 = x;
    v0 = v;

    object->GetPosition() = x;
    object->GetVelocity() = v;

    //compute rotation: valid only for sphere rolling on a plane (only the pool balls are moving so it's ok)

    // rotation propagation via quaternions:
    // omega_dot = inv(I) * ( M - omega x Iomega  )
    // omega = integ(omega_dot dt)
    // angle = integ(omega dt)
    // Q_dot = 0.5 * Q * OMEGA -> OMEGA quaternion from omega, 4th element = 0
    // integrate Q_dot
    // note M and I are in body-fixed coordinates

    float v_l = v.GetLength();
    CVector3 v_u;

    if (v_l != 0.0)
        v_u = v * ( 1.0 / v_l );
    else
        v_u =  object->GetHeading(); //if the ball is at rest, set the new heading to the previous one

    object->GetHeading() = v_u;
    object->GetRotationAxis() = v_u.Cross(CVector3(0.0, 0.0, 1.0));

    // TODO Hack! in this particular case we know that only balls can move,
    // but we need to switch this to a more general attitude propagation
    // using quaternions!
    float radius = dynamic_cast<CSphere*>(object->GetCollisionShape().get())->GetRadius();

    object->GetRotationAngle() += ( - v_l / radius )*mTimeStep;
 }

CVector3 CPoolPhysics::Derivatives(CVector3& pos, CVector3& vel, float mass, float CD, float Ae, bool falling)
{
    CVector3 acceleration;

    float V =  vel.GetLength();

    CVector3 velDir;

    if (V != 0.0)
        velDir = vel * ( 1.0 / V );

    float maxFriction = mass * (-mGravityAcceleration.z()) * mFrictionCoefficient;
    float aeroDrag = 0.5 * ( CD * Ae / mass ) * mAirDensity * (V*V);

    CVector3 friction, drag;

    friction = velDir * (-maxFriction);
    drag = velDir * (-aeroDrag);

    if (!falling)
        acceleration.SetComponents( friction.x() + drag.x() , friction.y() + drag.y(), 0.0 );
    else
        acceleration.SetComponents( friction.x() + drag.x(), friction.y() + drag.y(), mGravityAcceleration.z() );

    return acceleration;
}

void CPoolPhysics::ElasticCollision(CollisionPair &c_pair)
{
	CollisionShapePtr shape1 = c_pair.first;
	CollisionShapePtr shape2 = c_pair.second;
	
    switch (shape1->GetType())
    {
    case CS_SPHERE:
    	switch (shape2->GetType())
    	{
    	case CS_SPHERE:
    		Sphere2SphereCollision( c_pair );
    		break;
    	case CS_PLANE:
    		Sphere2PlaneCollision( c_pair );
    		break;
    	default:
    		break;
    	}
    	break;
    case CS_PLANE:
    	switch (shape2->GetType())
    	{
    	case CS_SPHERE:
    		Sphere2PlaneCollision( c_pair );
    		break;
    	case CS_PLANE:
    		CLogManager::getSingleton()->logMessage("Error: PLANE/PLANE Collisions are not supported.");
    		break;
    	default:
    		break;
    	}
    	break;
    default:
    	break;
    }

}

void CPoolPhysics::Sphere2SphereCollision(CollisionPair &c_pair)
{
	IObject *obj1, *obj2;

	obj1 = c_pair.first->GetParent();
	obj2 = c_pair.second->GetParent();

    /*If one of the two spheres is marked as falling in a pocket, it means that is colliding with
    a sphere that already is in a pocket. Since the two spheres then are clearly out of the game,
    ignore this collision. This has three effects: 1) no collision sound is played for pocketed balls,
    otherwise an endless sound would be played. 2) balls in pockets can penetrate each other, avoiding
    piles of balls coming out of pockets. 3) exchanging momentum between falling balls can have the nasty
    effect of sending balls flying through the air.*/
    if ( obj1->IsFalling() || obj2->IsFalling() )
        return;

    //find the vector normal to the tangent plane at the collision point.
    //since the colliding objects are 2 spheres, the normal vector is simply
    //the difference between the two centers.
    CVector3 normal( obj1->GetPosition() - obj2->GetPosition() );
    CVector3 relVel( obj1->GetVelocity() - obj2->GetVelocity() );

    //find the normal unit vector
    CVector3 normal_u( normal * ( 1.0 / normal.GetLength() ) );
    CVector3 relVel_u( relVel * ( 1.0 / relVel.GetLength() ) );

    float check_dir = relVel_u.Dot(normal_u);

    //the two balls are travelling away from each other, ignore this collision
    if (check_dir > 0)
        return;

    /*collisions happen only in-plane, there should be no vertical component.
    there are instanes in which numerical errors and/or floating point approximations
    lead to a non-null vertical component. this must not happen or else the exchange of momentum
    will have a veritcal component, leading to the balls floating through the air.
    As a precaution, make sure the vertical component is null always.*/
    if (normal_u.z() != 0.0 )
        normal_u.SetComponents( normal_u.x(), normal_u.y(), 0.0 );

    //find the (scalar) normal components of both velocities vectors. i.e. sphere.velocity dot normal_u
    float v1_n = obj1->GetVelocity().Dot( normal_u );
    float v2_n = obj2->GetVelocity().Dot( normal_u );

    //perform the exchange in momentum along normal_u
    float m1 = obj1->GetMass();
    float m2 = obj2->GetMass();

    float v1_n2 = ( v1_n * (m1 - m2) + 2.0*m2*v2_n ) /
                  ( m1 + m2 );

    float v2_n2 = ( v2_n * (m2 - m1) + 2.0*m1*v1_n ) /
                  ( m1 + m2 );

    //find the tangential (vector) component for both velocities
    //tangential components are unaltered by the collision.
    CVector3 v1_tan( obj1->GetVelocity() - normal_u*v1_n );
    CVector3 v2_tan( obj2->GetVelocity() - normal_u*v2_n );

    //construct the new velocity vectors by summing the old tangential components and the new
    //normal components
    CVector3 newV1( normal_u*v1_n2 + v1_tan );
    CVector3 newV2( normal_u*v2_n2 + v2_tan );

    //save results
    obj1->GetVelocity() = newV1;
    obj2->GetVelocity() = newV2;

     //compute collision point, all balls are the same size therefore the collision pioint will be in the middle
    //of the "normal" vector (which is the distance between the centers of the two colliding spheres)
    //collision point coordinates relative to sphere2
    CVector3 collPoint = normal_u * (normal.GetLength() / 2.0);
    //collison point in world coordinates
    collPoint = collPoint + obj2->GetPosition();

    EventPtr bb_event( new CBallCollisionEvent( obj1->GetObjectID(), obj2->GetObjectID(), collPoint, normal_u ) );
    CEventManager::getSingleton()->queueEvent(bb_event);

}

void CPoolPhysics::Sphere2PlaneCollision(CollisionPair &c_pair)
{
	IObject *plane, *sphere;
	CPlane* csp;
	CSphere* css;

	/*if ( ( obj1->GetCollisionShape()->GetType() == CS_SPHERE ) &&
			( obj2->GetCollisionShape()->GetType() == CS_PLANE ) )
	{
		plane = obj2;
		sphere = obj1;
	}
	else
	{
		plane = obj1;
		sphere = obj2;
	}*/

	plane = c_pair.second->GetParent();
	sphere = c_pair.first->GetParent();

	csp = (CPlane*)c_pair.second.get();
	css = (CSphere*)c_pair.first.get();

    //ignore collisions with the pool table bed.
	//TODO change!
    if (plane->GetName() == "slate")
        return;

    //as in the sphere/sphere collision, if a ball is falling in a pocket ignore this collision.
    if (sphere->IsFalling())
        return;

    //Note: colliding planes are considered to be always at rest (not moving)
    //and with no mass. Therefore, to simplify things, the effect of colliding
    //with a plane is simply a change in the direction of the normal component
    //of the initial velocity.

    //in this case, the tangent plane at the collision point is the colliding plane.
    //therefore, the normal unit vector needed is simply the normal of the plane.

    CVector3 normal_u( csp->GetNormal() );
    CVector3 v(sphere->GetVelocity());
    float v_norm = v.GetLength();

    // make sure the velocity is not 0
    if (v_norm == 0)
    	return;

    //check in which direction is travelling the ball w.r.t. the colliding plane
    CVector3 v_u = v / v_norm;

    float check_dir = normal_u.Dot(v_u);

    //if the ball is travelling away from the plane ignore collision.
    if (check_dir > 0)
        return;

    //find the scalar projection on the normal of the sphere velocity
    float v_n = sphere->GetVelocity().Dot( normal_u );

    //as before, the tangent component is unaltered.
    CVector3 v_tan( sphere->GetVelocity() - normal_u*v_n );

    //change the normal component direction
    float v_n2 = -v_n;

    CVector3 newV( normal_u*v_n2 + v_tan );

    //save results
    sphere->GetVelocity() = newV;

    //compute collision point, approx. sphere radius length in the direction of normal_u
    //directly in world coordinates
    CVector3 collPoint = sphere->GetPosition() + normal_u * css->GetRadius();

    EventPtr tb_event( new CTableCollisionEvent( sphere->GetObjectID(), collPoint, normal_u ) );
    CEventManager::getSingleton()->queueEvent(tb_event);

}
